Least Squares Plane Fit

This case study demonstrates the calculation of the best-fit plane to a set of input points using a least squares approach. The source code is written in C++ and uses the linear algebra package Armadillo to perform a singular value decomposition of a co-ordinate matrix.

Some Mathematics

Given a set of N points in $\mathbb{R}^3$ $\{\boldsymbol{p_i}: i=1,N\}$, the problem is expressed mathematically as; find a unit vector $\boldsymbol{n}$ that minimizes: $$ \sum_{i=1}^{N} (\boldsymbol{p_i}.\boldsymbol{n})^2 $$ where, without loss of generality, we assume that the points are centered at the origin: $$ \sum_{i=1}^{N} \boldsymbol{p_i} = \boldsymbol 0 $$

We therefore seek to minimize the objective function $\phi(\boldsymbol{n})$: $$ \phi(\boldsymbol{n}) = \lvert\lvert X^t \boldsymbol{n} \rvert\rvert^2 $$ where, $$ X = \lbrack \, \boldsymbol{p_1}, ..., \boldsymbol{p_N} \, \rbrack $$

The singular value decomposition theorem tells us that there exist a $3 \times 3$ orthogonal matrix $U$ and an $N \times N$ orthogonal matrix $V$ such that: $$ X = U \Lambda V $$ where $\Lambda$, a $3 \times N$ matrix is given by, $$ \Lambda = \begin{bmatrix} {\begin{matrix} \lambda_1&0&0 \\ 0&\lambda_2&0 \\ 0&0&\lambda_3 \end{matrix}} & \bigg\lvert & 0_{3 \times (N-3)} \end{bmatrix} $$ with $\lambda_1$, $\lambda_2$ and $\lambda_3$ being the left eigenvalues of $X$.

We can now write: $$ X^t \boldsymbol{n} = V^t \Lambda^t U^t \boldsymbol{n} = V^t \boldsymbol{q} $$ where, $$ \boldsymbol{q} = \Lambda^t U^t \boldsymbol{n} = \begin{bmatrix} \lambda_1 \boldsymbol{u_1}.\boldsymbol{n} \\ \lambda_2 \boldsymbol{u_2}.\boldsymbol{n} \\ \lambda_3 \boldsymbol{u_3}.\boldsymbol{n} \\ - \\ \boldsymbol{0} \end{bmatrix} $$ and $\boldsymbol{u_1}$, $\boldsymbol{u_2}$, $\boldsymbol{u_3}$ are the column vectors of $U$.

Given that $V$ is an orthogonal matrix, $\lVert V \boldsymbol{x}\rVert = \lVert\boldsymbol{x}\rVert$ $\forall \boldsymbol{x} \in \mathbb{R}^3$. Consequently, $$ \lVert X^t \boldsymbol{n} \rVert^2 = \lVert V^t \boldsymbol{q} \rVert^2 = \lVert \boldsymbol{q} \rVert^2 = \lambda_1^2 (\boldsymbol{u_1}.\boldsymbol{n})^2 + \lambda_2^2 (\boldsymbol{u_2}.\boldsymbol{n})^2 + \lambda_3^2 (\boldsymbol{u_3}.\boldsymbol{n})^2. $$

Given that $\boldsymbol{u_1}$, $\boldsymbol{u_2}$, $\boldsymbol{u_3}$ are the column vectors of $U$, they form an orthonormal basis for $\mathbb{R}^3$. We can therefore find $\mu_1$, $\mu_2$ and $\mu_3$ such that: $$ \boldsymbol{n} = \mu_1 \boldsymbol{u_1} + \mu_2 \boldsymbol{u_2} + \mu_3 \boldsymbol{u_3}.$$ and because $\boldsymbol{n}$ is a unit vector; $$ \mu_i^2 + \mu_j^2 + \mu_k^2 = 1. $$

Suppose that $\lambda_i$ is the minimum left eigenvalue for $X$. Then: $$ \begin{align*} \lVert X^t \boldsymbol{n} \rVert^2 & = (\lambda_i \mu_i)^2 + (\lambda_j \mu_j)^2 + (\lambda_k \mu_k)^2 \\ & = \lambda_i^2 (\mu_i^2 + \mu_j^2 + \mu_k^2) + (\lambda_j^2 - \lambda_i^2) \mu_j^2 + (\lambda_k^2 - \lambda_i^2) \mu_k^2 \\ & = \lambda_i^2 + (\lambda_j^2 - \lambda_i^2) \mu_j^2 + (\lambda_k^2 - \lambda_i^2) \mu_k^2 \\ & \ge \lambda_i^2 \end{align*} $$

$X^t \boldsymbol{n}$ is minimized if we choose $\boldsymbol{n} = \boldsymbol{u_i}$ where $\boldsymbol{u_i}$ is the eigenvector associated with $\lambda_i$, the minimal left eigenvalue for $X$.

Source Code

The source code is available for download here:

Plane Fitting.zip

The source code frequently uses objects of the V3 class, defined here in V3.h and implemented in V3.cpp. A V3 object is a 3d vector of type double complete with a number of useful mathematical member functions such as addition, subtraction, scalar product, vetor product. These operations are defined using operator overloading to provide a more intuitive read. Here are some typical exmaples of their use:


V3 u = { 0.5707, 0.5707, 0.5707 }; 

V3 v = { 1.0, 0.0, 0.0 }; 

V3 w; 



w = u + v;   // addition  
 
double d = u * v;   // dot product  
 
w = u ^ v;  // cross product

Note that, as is the case with a mathematical vector, a V3 object is used to contain a direction as well as a location.

The calculation of the best-fit plane is performed in the function PlaneFromPoints() using a singular value decomposition. The function accepts a constant vector of V3 objects and thence calculates the best fit plane in the form n.x = d where n is of the form double n[3] and d is of type double, with n and d being output parameters to PlaneFromPoints().